import numpy as np

from .control_assignment import ControlAssignment


class XControlAssignment(ControlAssignment):
    def __init__(self, dx: float, dy: float, ct: float, cm: float) -> None:
        super().__init__()

        self._P = np.diag([ct, ct, ct, cm])

        self._M0 = np.array([
            [1, 1, 1, 1],
            [-dx, dx, dx, -dx],
            [dy, dy, -dy, -dy],
            [1, -1, 1, -1]
        ])

        self._M0inv = np.linalg.inv(self._M0)
        self._Pinv = np.linalg.inv(self._P)
